#include "ros/ros.h"
#include "std_msgs/String.h"
#include "std_msgs/Int32.h"
#include "std_msgs/Empty.h"
#include "tf/transform_datatypes.h"
//#include "sensor_msgs/Imu.h"
//#include "px4_control_cfg.h"
#include "geometry_msgs/Twist.h" 
//#include "nlink_parser/LinktrackAnchorframe0.h"
//#include "nlink_parser/LinktrackTagframe0.h"
#include "geometry_msgs/PoseStamped.h"
//#include "tello_driver/TelloStatus.h"
#include <math.h>
#include <iostream>
#define PI 3.1415926
//#include <Eigen/Dense>
//#include <Eigen/Core>
using namespace std;
//using namespace Eigen;
// void cb_readPos_6(const geometry_msgs::PoseStamped::ConstPtr& msg)
// {
// 	geometry_msgs::PoseStamped pos;
// 	pos = *msg;

// 	tello_pos6[0][0] = pos.pose.position.x;
// 	tello_pos6[1][0] = pos.pose.position.y;
// 	tello_pos6[2][0] = pos.pose.position.z;
//     ROS_INFO("飞机位置信息:x %.2f,y %.2f,z %.2f",tellopose6[0][0],tello_pos6[1][0],tello_pos6[2][0]);
// }
int main(int argc, char *argv[])
{
    ros::init(argc,argv,"tello_control");
    ros::NodeHandle nh;
    ros::Publisher vel_pub1 = nh.advertise<geometry_msgs::Twist>("/tello6/cmd_vel",10);
    // ros::Subscriber pos_sub6 = nh.subscribe("/tello6/pose", 10, cb_readPos_6);
    // 4.循环发布运动控制消息
    //4-1.组织消息
    geometry_msgs::Twist cmd_vel;
    cmd_vel.linear.x = 0.;
    cmd_vel.linear.y = 0.0;
    cmd_vel.linear.z = 0.0;

    cmd_vel.angular.x = 0.0;
    cmd_vel.angular.y = 0.0;
    cmd_vel.angular.z = 0.5;

    //4-2.设置发送频率
    ros::Rate r(10);
    //4-3.循环发送
    while (ros::ok())
    {
        vel_pub1.publish(cmd_vel);
        // vel_pub1.publish(msg);

        ros::spinOnce();
    }
    return 0;
}


